// RobotPose.js

class RobotPose {
    constructor() {
        // 米为单位的实际物理坐标
        this.x = 0;           // 米
        this.y = 0;           // 米
        this.theta = 0;       // 弧度

        // THREE.js世界坐标
        this.worldX = 0;      // THREE.js单位
        this.worldZ = 0;      // THREE.js单位
    }

    // 更新物理坐标并同步世界坐标
    updatePose(x, y, theta) {
        this.x = x;
        this.y = y;
        this.theta = theta;
        this.updateWorldCoords();
    }

    // 更新世界坐标
    updateWorldCoords() {
        this.worldX = worldUnitsManager.metersToUnits(this.x);
        this.worldZ = worldUnitsManager.metersToUnits(this.y);
    }

    // 获取当前位姿
    getPose() {
        return {
            x: this.x,
            y: this.y,
            theta: this.theta,
            worldX: this.worldX,
            worldZ: this.worldZ
        };
    }

    // 获取世界坐标
    getWorldCoords() {
        return {
            x: this.worldX,
            z: this.worldZ
        };
    }

    // 重置位姿
    reset() {
        this.x = 0;
        this.y = 0;
        this.theta = 0;
        this.updateWorldCoords();
    }
}

// 创建全局单例
const robotPose = new RobotPose();

// 导出到全局作用域
if (typeof window !== 'undefined') {
    window.robotPose = robotPose;
    window.RobotPose = RobotPose;
}

// CommonJS 模块导出
if (typeof module !== 'undefined' && module.exports) {
    module.exports = {
        RobotPose,
        robotPose
    };
}